//
// Created by subject on 2025/6/19.
//


#include "ImuManager.h"

int ImuManager::startImu() {
    std::lock_guard<std::mutex> lock(imuMtx);
    if (!isImuOn) {
        isImuOn = true;
        imuThread = std::thread(&ImuManager::threadFunction, this);
        imuThread.detach(); // 分离线程，允许其独立运行
        LOGI("IMU started.");
    } else {
        LOGI("IMU is already running.");
    }
    return IMU_SUCCESS;
}

int ImuManager::stopImu() {
    {
        std::lock_guard<std::mutex> lock(imuMtx);
        if (!isImuOn) {
            // 已经是关闭状态，返回 IMU_ALREADY_STOPPED
            LOGI("IMU is already stopped.");
            return IMU_SUCCESS;
        }
        isImuOn = false; // 设置为关闭
    }
    cv.notify_all(); // 唤醒线程，以便其可以检查新状态
    std::cout << "IMU stopped." << std::endl;
    return IMU_SUCCESS;
}

ImuManager::ImuManager() : isImuOn(false) {
    // 初始化
    NxrMoudle moudle;
    bool ret = moudle.open("libnxrsensor_client.so");
    if (ret) {
        _vsc = (INxrSensorClient *) moudle.getNxrClient();
    }
    // vsc = new NativeVSC();
    if (!ret || _vsc == NULL) {
        LOGE("Open nxr_sensor_client failed!\n");
    }
    _vsc->initVSC(); //初始化传感器
    LOGI("Open nxr_sensor_client success!\n");
}

ImuManager::~ImuManager() {
    clearAllCallbacks();
    stopImu();
    if (_vsc != NULL) {
        _vsc->uninitVSC();
        _vsc = NULL;
    }
    LOGI("Close nxr_sensor_client success!\n");
}

void ImuManager::threadFunction() {
    NxrRawData imu_data;
    LOGI("OnImuCallback thread started.\n");

    while (true) {
        {
            std::unique_lock<std::mutex> lock(imuMtx);
            cv.wait(lock, [this]() { return !isImuOn; }); // 等待信号
            if (!isImuOn) {
                break; // 如果 IMU 被关闭，退出循环
            }
            // 获取 IMU 数据
            _vsc->getNxrRawData(&imu_data);
            callCallbacks(imu_data);
            LOGD("IMU Data: ");
        } // 离开作用域，自动解锁
        // 处理数据间隔（可以设置为所需的延迟）
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
    LOGI("OnImuCallback thread exiting.\n");
}

void ImuManager::registerCallback(const std::function<void(NxrRawData)> &callback) {
    if (!callback) {
        LOGE("Attempt to register a null callback."); // 使用 LOGE 输出错误信息
        return; // 如果为空，直接返回
    }
    std::lock_guard<std::mutex> lock(callbackMtx);
    callbackQueue.push(callback);
    LOGI("register IMU Callback!");
}

void ImuManager::removeCallback(const std::function<void(NxrRawData)> &callback) {
    if (!callback) {
        LOGE("Attempt to remove a null callback."); // 使用 LOGE 输出错误信息
        return; // 如果为空，直接返回
    }
    std::lock_guard<std::mutex> lock(callbackMtx);
    std::queue<std::function<void(NxrRawData)>> tempQueue;

    // 移除匹配的回调
    while (!callbackQueue.empty()) {
        auto currCallback = callbackQueue.front();
        callbackQueue.pop();
        if (currCallback.target<void(NxrRawData)>() != callback.target<void(NxrRawData)>()) {
            tempQueue.push(currCallback); // 只保留不匹配的回调
        }
    }
    callbackQueue = std::move(tempQueue); // 更新回调队列
    LOGI("Callback removed.");
}

void ImuManager::clearAllCallbacks() {
    std::lock_guard<std::mutex> lock(callbackMtx);
    while (!callbackQueue.empty()) {
        callbackQueue.pop();
    }
    LOGI("All callbacks cleared.");
}

void ImuManager::callCallbacks(const NxrRawData &data) {
    std::lock_guard<std::mutex> lock(callbackMtx);
    auto tempQueue = callbackQueue; // 创建副本以防止修改正在迭代的队列

    while (!tempQueue.empty()) {
        auto callback = tempQueue.front();
        tempQueue.pop(); // 取出回调，防止无限循环
        callback(data); // 执行回调
    }
}
